// ADIS 16480 Arduino Code
// Reads Yaw, Pitch, and Roll data from the IMU
// Prints to Serial after recieving 'D' Character
//
//


#include "ADIS16364.h"
#include <SPI.h>

ADIS16364 iSensor(10);
#define CS 10 //CS pin 10

signed int xVal, yVal, zVal;
word x_low, x_high, y_low, y_high, z_low, z_high;
int loopCount = 0;


void setup (){
  // Start serial
  Serial.begin(9600);
  
  iSensor.low_power_mode();

  delay(100);  
}

void loop(){
  // If serial has received 
  if(Serial.available() > 0){   
    // if the recieved character is 'D'
      if(Serial.read() == 'D'){
          printInfo();
      }
  }
  
  readAngles();
  //delay(500);
  //printInfo();
  
}
void readAngles()
{
    x_low = reg_read(0x1C);
    x_high = reg_read(0x6A);
    y_low = reg_read(0x20);
    y_high = reg_read(0x6C);
    z_low = reg_read(0x24);
    z_high = reg_read(0x6E);
    
    xVal = x_high;
    yVal = y_high;
    zVal = z_high;
}
void printInfo()
{
    Serial.print(xVal);
    Serial.print("\t");
    Serial.print(yVal);
    Serial.print("\t");
    Serial.print(zVal);
    Serial.print("\n");
}  

signed int twos_comp(unsigned int bits, unsigned int num)
{
    if(num>>16 & 0x01)
    {
      return 1;
    }
    else
    {
      return 1;
    }
}

word reg_read(unsigned char reg){
  // initialize variables
  word upper, lower;
 
  // Send register address
  digitalWrite(CS, LOW);
  SPI.transfer(reg);
  SPI.transfer(0x00);
  digitalWrite(CS, HIGH);
 
  // delay for one cycle, since you're using 4 MHz clock
  // it should be 0.25 us, but the smallest delay possible for delayMicroseconds() is 1 us
  delayMicroseconds(1);
 
  // get upper and lower result
  digitalWrite(CS, LOW);
  upper = SPI.transfer(0x00);
  lower = SPI.transfer(0x00);
  digitalWrite(CS, HIGH);
 
  // Combine upper and lower, and return
  return ( ( upper ) << 8 ) | ( lower );
}
